clc; clear; close all;

%% 无人机参数
Ix = 0.05; Iy = 0.05; Iz = 0.1;  % 转动惯量 (kg·m^2)
l = 0.2;                          % 旋翼到质心的距离 (m)
kappa = 0.01;                     % 旋翼扭矩系数

%% 状态空间模型 (线性化的小角度模型)
A = [zeros(3), eye(3); zeros(3,6)]; % 状态矩阵
B = [zeros(3,4); 
     0, l/Ix, 0, -l/Ix;             % 输入矩阵
     -l/Iy, 0, l/Iy, 0;
     kappa/Iz, -kappa/Iz, kappa/Iz, -kappa/Iz];
C = [eye(3), zeros(3)];             % 输出矩阵（测量欧拉角）
D = zeros(3,4);
sys = ss(A, B, C, D);               % 状态空间系统
%% 修正观测器设计（避免极点重复）
sys_poles = pole(sys);
% 手动指定观测器极点（比系统极点快，且互不相同）
obs_poles = [-2.5, -2.6, -2.7, -3.5, -3.6, -3.7]; 

% 检查能观性
Ob = obsv(A, C);
if rank(Ob) == size(A,1)
    try
        L = place(A', C', obs_poles)'; % 尝试place
    catch
        L = acker(A', C', obs_poles)'; % 退回到acker
    end
else
    error('System is not observable. Redesign the output matrix C.');
end
%% 模拟无故障数据以计算残差协方差矩阵
dt = 0.01; T_train = 5; t_train = 0:dt:T_train;
N_train = length(t_train);
residuals_normal = zeros(3, N_train);

x_true = zeros(6,1); x_est = zeros(6,1);
for k = 1:N_train
    u = [5; 5; 5; 5];               % 正常推力指令
    x_true = x_true + (A*x_true + B*u) * dt;
    y_true = C*x_true + 0.01*randn(3,1); % 带噪声的测量
    x_est = x_est + (A*x_est + B*u + L*(y_true - C*x_est)) * dt;
    residuals_normal(:,k) = y_true - C*x_est;
end
S = cov(residuals_normal');         % 残差协方差矩阵
S_inv = inv(S);                     % 协方差逆矩阵
%% 注入故障并在线检测
T = 10; t = 0:dt:T; N = length(t);
residuals = zeros(3, N);
mahalanobis_dist = zeros(1, N);
fault_detected = zeros(1, N);
chi2_threshold = 7.815;             % alpha=0.05, df=3

x_true = zeros(6,1); x_est = zeros(6,1);
for k = 1:N
    % --- 注入故障（5秒后旋翼1失效）---
    u = [5; 5; 5; 5];
    if t(k) >= 5
        u(1)=0;
        u(3) = 0;                   % 旋翼1完全失效
    end
    
    % --- 更新真实系统和观测器 ---
    x_true = x_true + (A*x_true + B*u) * dt + sqrt(0.001)*randn(6,1)*dt;
    y_true = C*x_true + 0.01*randn(3,1);
    x_est = x_est + (A*x_est + B*u + L*(y_true - C*x_est)) * dt;
    
    % --- 卡方检测 ---
    residuals(:,k) = y_true - C*x_est;
    mahalanobis_dist(k) = residuals(:,k)' * S_inv * residuals(:,k);
    if mahalanobis_dist(k) > chi2_threshold
        fault_detected(k) = 1;
    end
end
%% 可视化
figure;
subplot(2,1,1);
plot(t, mahalanobis_dist, 'b', 'LineWidth', 1.5); hold on;
plot(t, chi2_threshold * ones(size(t)), 'r--', 'LineWidth', 1.5);
%plot(t, fault_detected * max(mahalanobis_dist), 'g.', 'Markersize', 10);
title('卡方检测结果');
xlabel('时间 (s)'); ylabel('马氏距离');
legend('马氏距离', '卡方阈值');
grid on;

subplot(2,1,2);
plot(t, residuals', 'LineWidth', 1.5);
title('残差信号');
xlabel('时间 (s)'); ylabel('残差');
legend('\phi', '\theta', '\psi');
grid on;